/*
 * example6.c
 *
 * Created: 8/29/2013 11:41:01 PM
 *  Author: Tim
 */ 


#include "src/Epiphany.h"

#define usToTmrClks(uSeconds)	(uSeconds/2)

#define servoCenter				(usToTmrClks(1500))

#define servoMin				(servoCenter - (usToTmrClks(10)*90))

uint8_t i;

void servoSetup();
void setServoAngle(uint8_t servoNumber,uint8_t angle);
		
void setup()
{
	//initialize system clock to 32MHz
	clockInit();

	servoSetup();
	
	RTC_DelayInit();

	//global interrupt enable
	sei();
	
}

void loop()
{

	for(i=0;i<180;)
	{
		setServoAngle(0,i);
		
		if(delayOver)
		{
			RTC_Delay_ms(20);
			i++;
		}
	}
	
	for(i=180;i>0;)
	{
		setServoAngle(0,i);
		
		if(delayOver)
		{
			RTC_Delay_ms(20);
			i--;
		}
	}
}

void servoSetup()
{	
	TCD0.CTRLA		=	TC_CLKSEL_DIV64_gc;
	TCD0.PER		=	10000;
	TCD0.CTRLB		= 
						TC0_CCAEN_bm	|
						TC0_CCBEN_bm	|
						TC0_CCCEN_bm	|
						TC0_CCDEN_bm	|
						TC_WGMODE_SS_gc	;
	
	PORTD.DIRSET	=	0x0F;
	
	TCD0.CCA = servoCenter;
	TCD0.CCB = servoCenter;
	TCD0.CCC = servoCenter;
	TCD0.CCD = servoCenter;
	
	PORTD.DIRSET = 0xF;
					
}

void setServoAngle(uint8_t servoNumber,uint8_t angle)
{
	switch (servoNumber)
	{
		case 0:
			TCD0.CCA = servoMin + usToTmrClks(10)*angle;
		break;
		case 1:
			TCD0.CCB = servoMin + usToTmrClks(10)*angle;
		break;
		case 2:
			TCD0.CCC = servoMin + usToTmrClks(10)*angle;
		break;
		case 3:
			TCD0.CCD = servoMin + usToTmrClks(10)*angle;
		break;
	}
}